#include <iostream>
#include <opencv2/opencv.hpp>
#include "Aim/AngleSolver.hpp"
#include "Aim/ArmorDetector.hpp"
#include "Aim/separate.h"
#include "Aim/Settings.hpp"
#include "header.h"
#include "SerialPort/serialport.h"
#include "Camera/RMVideoCapture.hpp"


using namespace std;
using namespace cv;

int main(){
    SerialPort port("/dev/ttyUSB0");
    port.initSerialPort();

    // VideoCapture cap1;
    // VideoCapture cap0;

    RMVideoCapture cap1("/dev/video0",3);
    cap1.setVideoFormat(1280,720,1);
    cap1.info();
    cap1.setVideoFPS(60);
    cap1.setExposureTime(0,0);//change
    cap1.startStream();
    RMVideoCapture cap0("/dev/video1",3);
    cap0.setVideoFormat(1280,720,1);
    cap0.info();
    cap0.setVideoFPS(60);
    cap0.setExposureTime(0,0);//change
    cap0.startStream();

    cout<< "cap1: " << cap1.camnum <<endl;
    cout<< "cap0: " << cap0.camnum <<endl;

    RMVideoCapture cap_up,cap_down;
    if(cap0.camnum == 3){
        cap_up = cap0;
    }
    else if(cap0.camnum = 4){
        cap_down = cap0;
    }

    if(cap1.camnum == 3){
        cap_up = cap1;
    }
    else if(cap1.camnum = 4){
        cap_down = cap1;
    }

    
    

    // cap1.open(1);
    // cap0.open(0);
    // if(!cap1.isOpened() || !cap0.isOpened()){
    //     cout << "Camera can not be opened..." << endl;
    //     return -2;
    // }

    Settings setting(config_file_name);
    FileStorage fs1(setting.intrinsic_file_720, FileStorage::READ);
    while (!fs1.isOpened())	
    {
        cout << "Could not open the configuration file: \"" << setting.intrinsic_file_720 << "\"" << endl;
        return -1;
    }
    Mat cam_matrix_720, distortion_coeff_720;
    fs1["Camera_Matrix"] >> cam_matrix_720;
    fs1["Distortion_Coefficients"] >> distortion_coeff_720;
    /*===========================相机参数传入===========================*/

    /*===========================角度解算参数传入===========================*/
    // 最后不是根据这个的
    AngleSolver solver_720(cam_matrix_720, distortion_coeff_720, 22.5, 5.5);
    Point2f image_center_720 = Point2f(cam_matrix_720.at<double>(0,2), cam_matrix_720.at<double>(1,2));
    // 大小装甲板的角度解法
    AngleSolverFactory angle_slover1;
    AngleSolverFactory angle_slover0;
    angle_slover1.setTargetSize(22.5, 5.5, AngleSolverFactory::TARGET_ARMOR);
    angle_slover0.setTargetSize(22.5, 5.5, AngleSolverFactory::TARGET_ARMOR);
    angle_slover1.setTargetSize(13, 5.5, AngleSolverFactory::TARGET_SAMLL_ATMOR);
    angle_slover0.setTargetSize(13, 5.5, AngleSolverFactory::TARGET_SAMLL_ATMOR);
    /*===========================角度解算参数传入===========================*/

    /*===========================创建装甲板识别类===========================*/
    ArmorDetector armor_detector1(setting.armor); //此处是赋值给_Para 还不知道有啥用
    ArmorDetector armor_detector0(setting.armor);
    ArmorParam armor_para_720 = setting.armor;
    armor_detector1.setPnPSlover(&solver_720);
    armor_detector0.setPnPSlover(&solver_720);
    armor_detector1.setPara(armor_para_720);
    armor_detector0.setPara(armor_para_720);
    angle_slover1.setSolver(&solver_720);
    angle_slover0.setSolver(&solver_720);
    VisionData vdata1;
    VisionData vdata0;
    /*===========================创建装甲板识别类===========================*/

    /*===========================创建大神符识别类===========================*/
    // Detect Find;
    VisionData Vision;
    bool Rune_flag = false;
    /*===========================创建大神符识别类===========================*/

    /*===========================函数中所使用的参数===========================*/
    Mat src_csm;
    Mat src1,src0;
    int frame_num = 0;
    int near_face1 = 0;
    int near_face0 = 0;
    int miss_detection_cnt1 = 0;
    int miss_detection_cnt0 = 0;
    int find_cnt0 = 0;
    int find_cnt1 = 0;
    float last_x1 = 0, last_y1 = 0, last_dist1;
    float last_x0 = 0, last_y0 = 0, last_dist0;
    RotatedRect rect0,rect1;
    
    double distance1 = 0;
    double distance0 = 0;
    Point2f center1 = cv::Point2f();
    Point2f center0 = cv::Point2f();
    /*===========================函数中所使用的参数===========================*/

    int mode=1;

    while(1){
        double angle_x1 = 0.0, angle_y1 = 0.0, dist1 = 0;
       double angle_x0 = 0.0, angle_y0 = 0.0, dist0 = 0;
        if (mode == 1) {
            armor_detector1.enemy_color = RED;
            armor_detector0.enemy_color = RED;
        }
        else if (mode == 2) {
            armor_detector1.enemy_color = BLUE;
            armor_detector0.enemy_color = BLUE;
        }
        cap_up >> src1;
        cap_down >> src0;

        //图片数据正常
        if(src0.empty()) continue;
        if(src1.empty()) continue;
        if(src0.channels() != 3) continue;
        if(src1.channels() != 3) continue;

        //直接调用函数找到读进来的图中是否有目标
        rect1 = armor_detector1.getTargetAera(src1, 0, 0, 1);
        rect0 = armor_detector0.getTargetAera(src0, 0, 0, 0);
        center1= rect1.center;
        center0= rect0.center;
        int len1 = MIN(rect1.size.height, rect1.size.width);
        int len0 = MIN(rect0.size.height, rect0.size.width);

        if(len1 > armor_detector1.get_near_face()){
            near_face1 = 1;
        }else{
            near_face1 = 0;
        }
        if(len0 > armor_detector0.get_near_face()){
            near_face0 = 1;
        }else{
            near_face0 = 0;
        }

        // 根据长宽比判断目标是大装甲还是小装甲
        AngleSolverFactory::TargetType type1 = armor_detector1.isSamllArmor() ? AngleSolverFactory::TARGET_SAMLL_ATMOR : AngleSolverFactory::TARGET_ARMOR;
        AngleSolverFactory::TargetType type0 = armor_detector0.isSamllArmor() ? AngleSolverFactory::TARGET_SAMLL_ATMOR : AngleSolverFactory::TARGET_ARMOR;

        if(angle_slover1.getAngle(rect1, type1, angle_x1, angle_y1, dist1) == true)
            {
                miss_detection_cnt1 = 0;
                /**
                 * 这是当初为了做移动预测加的条件，已弃用
                 */
                int bias_w_l = rect1.size.width * (0.5 + 0.8);
                int bias_w_r = rect1.size.width * (0.5 + 0.8);
                int bias_h_u = rect1.size.height * (0.5 + 0.5);
                int bias_h_b = rect1.size.height * (0.5 + 0.2);
                int actual_center_x1 = 765;
                int actual_center_y1 = 291;
                if(rect1.center.x > actual_center_x1 - bias_w_l
                    && rect1.center.x < actual_center_x1 + bias_w_r
                    && rect1.center.y > actual_center_y1 - bias_h_u
                    && rect1.center.y < actual_center_y1 + bias_h_b)
                ++find_cnt1;

                else{
                    find_cnt1 = 0;
                }

            }
            else {
                ++miss_detection_cnt1;
                find_cnt1 = 0;
            }
            if (angle_x1 != (double)0 && angle_y1 != (double)0)
            {
                // 这是用excel拟合出来的灯条高度与实际距离的函数关系（我用单目pnp解出的距离不行）
                distance1 = 10941 * pow(len1, -1.066);
                if(find_cnt1 >= 2)
                        vdata1 = {(float)angle_x1, (float)angle_y1, (float)distance1, 1, 1, 0};
                    else
                        vdata1 = {(float)angle_x1, (float)angle_y1, (float)distance1, 0, 1, 0};
                    last_x1 = angle_x1;
                    last_y1 = angle_y1;
                    last_dist1 = distance1;
                port.TransformUpData(vdata1);
                port.UPsend();
            }
    






          if(angle_slover0.getAngle(rect0, type0, angle_x0, angle_y0, dist0) == true)
            {
                miss_detection_cnt0 = 0;
                /**
                 * 这是当初为了做移动预测加的条件，已弃用
                 */
                int bias_w_l = rect0.size.width * (0.5 + 0.8);
                int bias_w_r = rect0.size.width * (0.5 + 0.8);
                int bias_h_u = rect0.size.height * (0.5 + 0.5);
                int bias_h_b = rect0.size.height * (0.5 + 0.2);
                int actual_center_x0 = 765;
                int actual_center_y0 = 291;
                if(rect0.center.x > actual_center_x0 - bias_w_l
                    && rect0.center.x < actual_center_x0 + bias_w_r
                    && rect0.center.y > actual_center_y0 - bias_h_u
                    && rect0.center.y < actual_center_y0 + bias_h_b)
                ++find_cnt0;

                else{
                    find_cnt0 = 0;
                }

            }
            else {
                ++miss_detection_cnt0;
                find_cnt0 = 0;
            }
            if (angle_x0 != (double)0 && angle_y0 != (double)0)
            {
                // 这是用excel拟合出来的灯条高度与实际距离的函数关系（我用单目pnp解出的距离不行）
                distance0 = 10941 * pow(len0, -1.066);
                if(find_cnt0 >= 2)
                        vdata0 = {(float)angle_x0, (float)angle_y0, (float)distance0, 1, 1, 0};
                    else
                        vdata0 = {(float)angle_x0, (float)angle_y0, (float)distance0, 0, 1, 0};
                    last_x0 = angle_x0;
                    last_y0 = angle_y0;
                    last_dist0 = distance0;
                port.TransformDownData(vdata0);
                port.DOWNsend();
            }
    

        imshow("src1",src1);
        imshow("src0",src0);
        waitKey(1);
    }
    return 0;
}